#ifndef GMM_VOXEL_MAP_BASE_H
#define GMM_VOXEL_MAP_BASE_H

#include <Eigen/Core>
#include <iostream>
#include <vector>

namespace gvm {

template <typename _Datatype> class VoxelMapCircularBufferBase {
public:
  VoxelMapCircularBufferBase() {}
  ~VoxelMapCircularBufferBase() {}

  VoxelMapCircularBufferBase(double resolution, _Datatype empty_element,
                             const std::vector<int> &POW,
                             const std::vector<double> &centre_ratio)
      : N_AXIS({1 << POW[0], 1 << POW[1], 1 << POW[2]}), N_BACK(3, 0),
        N_CENTRE({static_cast<int>(N_AXIS[0] * centre_ratio[0]),
                  static_cast<int>(N_AXIS[1] * centre_ratio[1]),
                  static_cast<int>(N_AXIS[2] * centre_ratio[2])}),
        MASK({N_AXIS[0] - 1, N_AXIS[1] - 1, N_AXIS[2] - 1}),
        resolution_(resolution), empty_element_(empty_element),
        buffer_(N_AXIS[0] * N_AXIS[1] * N_AXIS[2], empty_element),
        mapCentreIdx(N_CENTRE[0], N_CENTRE[1], N_CENTRE[2]),
        mapCentrePos(0.0, 0.0, 0.0), offset(0,0,0),epsilon_vec3d(1e-6, 1e-6, 1e-6) {
    std::cout << "N_AXIS: " << N_AXIS[0] << " " << N_AXIS[1] << " " << N_AXIS[2]
              << std::endl;
    std::cout << "N_CENTRE: " << N_CENTRE[0] << " " << N_CENTRE[1] << " "
              << N_CENTRE[2] << std::endl;
    std::cout<<"MAP RANGE: "<<N_AXIS[0]* resolution_<<"m*"<<N_AXIS[1]*resolution_<<"m*"<<N_AXIS[2]*resolution_<<"m"<<std::endl;
  }
  std::vector<int> N_AXIS;
  std::vector<int> N_BACK;
  std::vector<int> N_CENTRE;
  std::vector<int> MASK;
  double resolution_;
  _Datatype empty_element_;
  std::vector<_Datatype> buffer_;
  Eigen::Vector3i mapCentreIdx;
  Eigen::Vector3d mapCentrePos; // discrete position in global coordinate
  Eigen::Vector3i offset;
  const Eigen::Vector3d epsilon_vec3d;

private:
  // This function maps the local index((0,0,0)~(N-1,N-1,N-1)) to the index in
  // Circular buffer vector.
  Eigen::Vector3i LocalIdx2CircularBufferIdx(const Eigen::Vector3i &index) {
    return Eigen::Vector3i((index(0) + N_BACK[0]) & MASK[0],
                           (index(1) + N_BACK[1]) & MASK[1],
                           (index(2) + N_BACK[2]) & MASK[2]);
  }
  // This function maps the 3d index(x,y,z) to the 1d index in
  // Circular buffer vector.
  int Convert3dIdxto1dIdx(int id_x, int id_y, int id_z) {
    return id_x + N_AXIS[0] * id_y + N_AXIS[0] * N_AXIS[1] * id_z;
  }
  int Convert3dIdxto1dIdx(const Eigen::Vector3i &index) {
    return index(0) + N_AXIS[0] * index(1) + N_AXIS[0] * N_AXIS[1] * index(2);
  }
  template <typename T> void MyPrint(std::string s, const T &eigen_vector) {
    std::cout << s << eigen_vector(0) << " , " << eigen_vector(1) << " , "
              << eigen_vector(2) << std::endl;
  }
  Eigen::Vector3i RoundEigenVectord(const Eigen::Vector3d& vec) {
    return Eigen::Vector3i(std::round(vec(0)), std::round(vec(1)),
                           std::round(vec(2)));
  }

public:
  Eigen::Vector3i GetOffset(){
    return offset;
  }
  inline bool insideRange(const Eigen::Vector3i &idx) {
    return (idx(0) < N_AXIS[0] && idx(1) < N_AXIS[1] && idx(2) < N_AXIS[2] &&
            idx(0) >= 0 && idx(1) >= 0 && idx(2) >= 0);
  }

  inline _Datatype getLocalIdxOccupiedProb(const Eigen::Vector3i &idx) {
    return at(idx);
  }
  // pos is the global position
  inline _Datatype getPosOccupiedProb(const Eigen::Vector3d &pos) {
    return at(getIdxLocal(pos));
  }

  Eigen::Vector3d getGlobalPosfromLocalIdx(const Eigen::Vector3i &idx) {
    return resolution_ * (idx - mapCentreIdx).cast<double>() + mapCentrePos;
  }
  // index is in a local idx. in local idx, the UAV is always the mapCentreIdx.
  inline _Datatype &at(const Eigen::Vector3i &index) {
    return buffer_[Convert3dIdxto1dIdx(LocalIdx2CircularBufferIdx(index))];
  }
  // get point index in local map  (0,0,0)~(N-1,N-1,N-1), point is in global map
  inline Eigen::Vector3i getIdxLocal(const Eigen::Vector3d &point) {
    return RoundEigenVectord((point + epsilon_vec3d - mapCentrePos) /
                             resolution_) +
           mapCentreIdx;
  }

  Eigen::Vector3i getClosestIdxInRange(const Eigen::Vector3d &originPos,
                                       const Eigen::Vector3d &endpoint) {
    Eigen::Vector3d diff = (endpoint - mapCentrePos);
    double min_t = DBL_MAX;
    int direction_index = 0;
    int sign = 1;
    for (int i = 0; i < 3; i++) {
      double ratio = 0.0;
      if (diff(i) > 0) {
        ratio = (N_AXIS[i] - N_CENTRE[i]) * resolution_ / double(diff(i));
      } else if (diff(i) < 0) {
        ratio = (N_CENTRE[i]) * resolution_ / double(-diff(i));
      } else {
        continue;
      }
      if (ratio < min_t) {
        min_t = ratio;
        direction_index = i;
        sign = diff(i) > 0 ? 1 : -1;
      }
    }
    double xb;
    if (sign > 0) {
      xb = mapCentrePos(direction_index) +
           sign * (N_AXIS[direction_index] - N_CENTRE[direction_index] - 1) *
               resolution_;
    } else {
      xb = mapCentrePos(direction_index) +
           sign * (N_CENTRE[direction_index] - 1) * resolution_;
    }
    double ratio = (xb - originPos(direction_index)) /
                   (endpoint(direction_index) - originPos(direction_index));
    Eigen::Vector3d closestPos = originPos + ratio * (endpoint - originPos);
    Eigen::Vector3i new_map_idx = getIdxLocal(closestPos);
    for(int i=0;i<3;i++){
      new_map_idx(i)=std::min(new_map_idx(i),N_AXIS[i]-1);
      new_map_idx(i)=std::max(new_map_idx(i),0);
    }
    return new_map_idx;
  }
  // new_originPos is a position in the world coordinate
  void updateMapOrigin(const Eigen::Vector3d& new_originPos) {
    // std::cout << "///////////" << std::endl;
    // std::cout << "start N_BACK: " << N_BACK[0] << " , " << N_BACK[1] << " ,"
    //           << N_BACK[2] << " &resolution_: " << &resolution_ << std::endl;
    // MyPrint("old mapCentrePos: ", mapCentrePos);
    // MyPrint("new_originPos: ", new_originPos);
    offset = RoundEigenVectord(
        (new_originPos + epsilon_vec3d - mapCentrePos) / resolution_);
    // MyPrint("offset: ", offset);
    for (int i = 0; i < 3; i++) {
      if (fabs(offset(i)) >= 1) {
        mapCentrePos[i] = mapCentrePos[i] + offset(i) * resolution_;
      }
    }
    // MyPrint("new mapCentrePos: ", mapCentrePos);
    // consider offset x direction
    if (offset(0) > 0) {
      for (int k = 0; k < N_AXIS[2]; k++) {
        for (int j = 0; j < N_AXIS[1]; j++) {
          for (int i = 0; i < std::min(offset(0), N_AXIS[0]); i++) {
            int clip_x = (N_BACK[0] + i) & MASK[0];
            buffer_[Convert3dIdxto1dIdx(clip_x, j, k)] = empty_element_;
          }
        }
      }
      N_BACK[0] = (N_BACK[0] + offset(0)) & MASK[0];
    } else if (offset(0) < 0) {
      int N_X_FRONT = (N_BACK[0] + N_AXIS[0] - 1) & MASK[0];
      for (int k = 0; k < N_AXIS[2]; k++) {
        for (int j = 0; j < N_AXIS[1]; j++) {
          for (int i = 0; i < std::min(-offset(0), N_AXIS[0]); i++) {
            int clip_x = (N_X_FRONT - i) & MASK[0];
            buffer_[Convert3dIdxto1dIdx(clip_x, j, k)] = empty_element_;
          }
        }
      }
      N_BACK[0] = (N_BACK[0] + offset(0)) & MASK[0];
    }
    // consider offset y direction
    if (offset(1) > 0) {
      for (int k = 0; k < N_AXIS[2]; k++) {
        for (int j = 0; j < std::min(offset(1), N_AXIS[1]); j++) {
          int clip_y = (N_BACK[1] + j) & MASK[1];
          for (int i = 0; i < N_AXIS[0]; i++) {
            buffer_[Convert3dIdxto1dIdx(i, clip_y, k)] = empty_element_;
          }
        }
      }
      N_BACK[1] = (N_BACK[1] + offset(1)) & MASK[1];
    } else if (offset(1) < 0) {
      int N_Y_LEFT = (N_BACK[1] + N_AXIS[1] - 1) & MASK[1];
      for (int k = 0; k < N_AXIS[2]; k++) {
        for (int j = 0; j < std::min(-offset(1), N_AXIS[1]); j++) {
          int clip_y = (N_Y_LEFT - j) & MASK[1];
          for (int i = 0; i < N_AXIS[0]; i++) {
            buffer_[Convert3dIdxto1dIdx(i, clip_y, k)] = empty_element_;
          }
        }
      }
      N_BACK[1] = (N_BACK[1] + offset(1)) & MASK[1];
    }
    // consider offset z direction
    if (offset(2) > 0) {
      for (int k = 0; k < std::min(offset(2), N_AXIS[2]); k++) {
        int clip_z = (N_BACK[2] + k) & MASK[2];
        for (int j = 0; j < N_AXIS[1]; j++) {
          for (int i = 0; i < N_AXIS[0]; i++) {
            buffer_[Convert3dIdxto1dIdx(i, j, clip_z)] = empty_element_;
          }
        }
      }
      N_BACK[2] = (N_BACK[2] + offset(2)) & MASK[2];
    } else if (offset(2) < 0) {
      int N_Z_UP = (N_BACK[2] + N_AXIS[2] - 1) & MASK[2];
      for (int k = 0; k < std::min(-offset(2), N_AXIS[2]); k++) {
        int clip_z = (N_Z_UP - k) & MASK[2];
        for (int j = 0; j < N_AXIS[1]; j++) {
          for (int i = 0; i < N_AXIS[0]; i++) {
            buffer_[Convert3dIdxto1dIdx(i, j, clip_z)] = empty_element_;
          }
        }
      }
      N_BACK[2] = (N_BACK[2] + offset(2)) & MASK[2];
    }
    // std::cout << "end N_BACK: " << N_BACK[0] << " , " << N_BACK[1] << " ,"
    //           << N_BACK[2] << " &resolution_: " << &resolution_ << std::endl;
    // std::cout << "///////////" << std::endl;
  }
};

} // namespace gvm

#endif